/*===============CLASE BUFFER 1.01
recibe bien las triadas xyz, probado a 10ms 
si se usa a otra velocidad 
*/


#define BAUDRATE B115200
#define MODEMDEVICE "/dev/ttyUSB0"
#define _POSIX_SOURCE 1 /* POSIX compliant source */
#define nTrama 5    


#include "buffer.h"
#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <unistd.h>  /* UNIX standard function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <time.h>
#include <iostream>
#include <cstdlib>
#include <strings.h>
#include <string.h>
#include <ctype.h>
#include <signal.h>


using std::cout;
using std::endl;

Buffer::Buffer()
{
}

void Buffer::initialize()
{
    for(int i=0; i<1000000;i++)
    {
        rx_buffer[i] = 0;
    }

    for(int i=0; i < 200000; i++)
    {
        values_arrayX[i] = 0;
        values_arrayY[i] = 0;
        values_arrayZ[i] = 0;
    }

    values_write_indexX = 0;
    values_read_indexX = 0;
    values_write_indexY = 0;
    values_read_indexY = 0;
    values_write_indexZ = 0;
    values_read_indexZ = 0;
    read_index=0;
    write_index=0;
    
    //cout << "incicializado buffer" << endl;
	algoritmosX.initialize('X',ts);
	algoritmosY.initialize('Y',ts);	
    continuar = true;
}

void Buffer::stopPort()
{
    continuar = false;
}

void Buffer::setSamplingRate(float _ts)
{
	ts = _ts;
	//algoritmosX.setSamplingRate(ts);
	//algoritmosY.setSamplingRate(ts);
}

int Buffer::validarToken(char* ptrVar)
{
	for(int i = 0; i < 3 ; i++)
	{
		/*cout << ptrVar[0+i*5] ;
		cout << ptrVar[1+i*5] ;
		cout << ptrVar[2+i*5] ;
		cout << ptrVar[3+i*5] ;
		cout << ptrVar[4+i*5] << "_" << endl;*/
	
		if( (isxdigit(ptrVar[1+i*5]) &&
    	    isxdigit(ptrVar[2+i*5]) &&
		    isxdigit(ptrVar[3+i*5]) &&
		    isxdigit(ptrVar[4+i*5])) == 1)
		{
		    int num = this->hexToInt(&ptrVar[1+i*5]);
	    
		    switch(ptrVar[0+i*5])
		    {
		        case 'x':
		            values_arrayX[values_write_indexX] = num;
		            values_write_indexX++;
					break;
		        case 'y':
		            values_arrayY[values_write_indexY] = num;
		            values_write_indexY++;		    		
	  		        break;
		        case 'z':
		            values_arrayZ[values_write_indexY] = num;
 	                values_write_indexZ++;
					break;
				default:
					cout << " error" << endl;
		    }
		}
		else
		{
		
					/*cout << "[";	
				for(int i = 0; i<5; i++) cout << ptrVar[i] ;
			    cout << "]"; 
			    cout << "NUMEROS MAL valor descartado"<< endl;*/
		    return 0;
		}
	}
	
		
	//	cout << values_arrayX[values_write_indexX-1] << " " ;
    //	cout << values_arrayY[values_write_indexY-1] << " ";	
    	//cout << values_arrayZ[values_write_indexZ-1] << " " << endl;				    	
						
		return 1;
}
	
void Buffer::procesar_trama(){

    char* ptrToken;
    char* ptrSubToken;
    unsigned int tokenLen=0;

    ptrToken = strchr(&rx_buffer[read_index],'x');

    if(ptrToken)
    {			//espera a que venga una x
        ptrSubToken = strchr(ptrToken+1,'x');	//ve si hay otra x
 	
 		//cout << "token "<< (int *)ptrToken<< "subtoken "<< (int *)ptrToken << endl;	


        if(ptrSubToken) //si no existen dos x estamos en el final del buffer o error
        {				//hay otra x
		   	//cout << "token " << ptrToken << endl;        
      		//cout << "subtoken "<<ptrSubToken << endl;
            tokenLen = ptrSubToken-ptrToken;
            //cout << "tkl " << tokenLen << endl;
           // cout << read_index<< "-" << tokenLen ;
            
            if(tokenLen >= nTrama*3)
            {
                read_index+=(nTrama *3); //menos 1 porque la x no es mas
										//la sumo ahora porque asi si hay un error
										//ya descarto el token entero

				//si llega aca quiere decir que tenomos una triada xyz
				validarToken(ptrToken);
				//calcula sobre los valores obtenidos
				algoritmosX.procesar( values_arrayX[values_write_indexX-1]);
				algoritmosY.procesar( values_arrayY[values_write_indexX-1]);				
//				algoritmos.procesar_y( values_arrayY[values_write_indexY-1]);
	//			algoritmos.procesar_z( values_arrayZ[values_write_indexZ-1]);				
			
	   			////cout << "[";	
				//for(int i = 0; i<nTrama*3; i++) cout << ptrToken[i] ;
			    //cout << "]"; 
			    //cout << endl;
			    			
			}
            //validarToken(ptrToken+1, 'x');	//pasa el valor sin la x
        }
    }
	
    if(read_index >= 1000000) read_index = 0;
}

//FUNCIONES DE LECTURA DEL PUERTO SERIE
int Buffer::hexToInt(const char* hexStr)
{
    int hexInt;
    sscanf(hexStr, "%x", &hexInt);

    return hexInt;
}

int Buffer::writeport(int fd, char *chars) {
    int len = strlen(chars);
    printf("Enviados: %dB\r\n",len);

    chars[len] = 0x0d; // stick a <CR> after the command
    chars[len+1] = 0x00; // terminate the string properly
    int n = write(fd, chars, strlen(chars));
    if (n < 0) {
        fputs("write failed!\n", stderr);
        return 0;
    }

    return 1;
}

int Buffer::initport(int fd) {
	struct termios newtio;
  		
    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
    newtio.c_iflag = IGNPAR;
    newtio.c_oflag = 0;
    
    /* set input mode (non-canonical, no echo,...) */
    newtio.c_lflag = 0;
     
    newtio.c_cc[VTIME]    = 0;   /* inter-character timer unused */
    newtio.c_cc[VMIN]     = 15;   /* blocking read until 5 chars received */
    
    tcflush(fd, TCIFLUSH);
    tcsetattr(fd,TCSANOW,&newtio);

    read_index = 0;
    write_index = 0;
    return 1;
}

int Buffer::readport(int fd, char *result, char echo_on) {
    int iIn = read(fd, result, 254);

    if (iIn < 0) {
        if (errno == EAGAIN) {
            //no hay datos todavia
            return 0;
        } else {
            printf("SERIAL read error %d %s\n", errno, strerror(errno));
            return 0;
        }
    }

    memcpy(&rx_buffer[write_index],result, iIn);
    write_index += iIn;									
  	rx_buffer[write_index] = 0x00;	//		para evitar errores de desborde

	if(echo_on)
	{
	 	for(int i = 0; i< iIn+2; i++)   
	 	{
	 		//cout << result[i] ;
	 		
	// 		flush(cout);
	 		
	 	}
	 	//cout << ": " << iIn << endl;
	} 		
 	
    if(write_index >= 1000000) write_index = 0;			//para que no se desborde

    return 1;
}

void Buffer::run()
{
	fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY ); 
    if (fd == -1) {
            perror("open_port: Unable to open" MODEMDEVICE);
            return;
    } 
    
    initport(fd);			//init serial port
	//cout << "conectado. Sampling Rate: " << getSamplingRate() << "s" << endl;
	
	 readport(fd,sResult,true);
    readport(fd,sResult,true);
    readport(fd,sResult,true);
    readport(fd,sResult,true);		   
		       		   
	cout << "OK" << endl;
	
	initialize();
	       
    while (continuar) {
  		//usleep(10000);
	    readport(fd,sResult,false);
		procesar_trama();
  		//if(write_index > 200)continuar = false;        
    }

	while((write_index-200)>read_index)		//procesa lo que falto
		procesar_trama();

	//printf("%s", &rx_buffer);
	//cout << endl <<write_index<< endl;
				
    close(fd);
    
    for(int i = 0; i<values_write_indexX; i++)
    {
    	//cout << values_arrayX[i] << " ";
    }
    
    return ;


	for(int i = 0; i<write_index; i++)
		{
			if(!(i%15))
			{
				/*printf(" %d",i);
				printf("\n");*/
				
			}
			//printf("%.2x ",(unsigned char)rx_buffer[i]);
		}
			/*cout << endl <<write_index;
			cout << endl;*/
		

    return ;
}
	
